/**
 * @file /src/qnode.cpp
 *
 * @brief  
 *
 * @date  
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/unicebot_gui/UnicebotNode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace unicebot_gui {

	static const char IMAGE_TOPIC_IN[] = "/camera/rgb/image_color";

/*****************************************************************************
** Implementation
*****************************************************************************/

UnicebotNode::UnicebotNode(int argc, char** argv, KinectWidget* pKinectWin ) :
			init_argc(argc),
			init_argv(argv)
	{
		ROS_WARN("UnicebotNode::UnicebotNode");
		ros::init(argc, argv, "unicebot_gui", ros::init_options::AnonymousName);
		this->pKinectWin=pKinectWin;
		stop=false;
	}



UnicebotNode::~UnicebotNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
	wait();
}

bool UnicebotNode::init() {
	ROS_WARN("UnicebotNode::init");
	//ros::init(init_argc,init_argv,"unicebot_gui");
	/*
	if ( ! ros::master::check() ) {
		return false;
	}
	//*/
	//ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.


	// Capturer les images kinect
	//
	imageConverter.init(n, IMAGE_TOPIC_IN, pKinectWin);

	/*
	image_transport::ImageTransport it(n); // voir http://code.ros.org/lurker/message/20091110.195525.b78e48ce.gl.html
	kinectWidget.sub_ = it.subscribe("/image", 1, KinectWidget::kinectCallback, this);
  	//kinectWidget.sub_ = it.subscribe("/image", 3, &imageCb);
	image_transport::Subscriber image_sub_ =  it.subscribe("/image", 1, &KinectWidget::kinectCallback, this);
	//*/

	// Démarrer le QThread (classe mere)
	//
	start();
	return true;
}



void UnicebotNode::run() {
	ROS_WARN("UnicebotNode::run");
	ros::Rate loop_rate(1);
	int count = 0;

	// Boucle principale d'evuenements du noeud
	//
	while ( ros::ok() && !stop)
	{

		// Eventuellement publier ici
		//
		ROS_WARN("###SPIN ONCE###");

		// Dispatcher les evenements ROS (un à la fois)
		//
		ros::spinOnce(); // différent de spin

		// un compteur histoire de ...
		//
		loop_rate.sleep();
		++count;
	}

	ROS_WARN("Ros shutdown, proceeding to close the gui");
	emit rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}


}  // namespace unicebot_gui
